/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/***********************************************************************
 *
 * FILENAME:
 * sockTest.cxx
 *
 * PURPOSE:
 *  Provide test of socket communication 
 *
 * LAST MODIFIED:
 * $Date: 2005/05/04 03:00:00 $
 * $Revision: 1.2 $
 * $Author: dixonc $
 ***********************************************************************/

#include <signal.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <fcntl.h>
#include <netdb.h>
#include <errno.h>

#include <string>
#include <iostream>

#include "socket.h"
#include "timeLib.h"
#include "command.h"
#include "queue.h"
#include "type_defs.h"

//
// Global Data
//
Socket *sock;
Socket *Bsock;
TimeStamp tsTx;
TimeStamp tsRx;
long long totalTime = 0;
unsigned long int minTime   = 999999999;
unsigned long int maxTime   = 0;

#define NUM_LOOP	128
Queue timeQueue(NUM_LOOP);

void exitsignal( int signal );
void clientMode();
void serverMode();
void closeConnection() ;
void parseClientCommand(Command &cmd);
void parseHostCommand(Command &cmd, int client);

//----------------------------------------------------------
// catch exit signals
//----------------------------------------------------------
void exitsignal( int signal )
{
	printf("\nCaught signal %d ... exiting\n",signal);
	fflush(stdout);
	closeConnection();	
	delete sock;
	delete Bsock;
	exit(EXIT_SUCCESS);
}

//----------------------------------------------------------
// Run like a Client
//----------------------------------------------------------
void clientMode()
{
	int client;
	int num;
	Command cmd;
	uint8_t *rxBuf = new uint8_t[ CMD_MAX_SIZE ];
	CommandQueue sockQueue;                         // parse incoming socket stream

	bool hostWait = 1;

	while(hostWait) {
	// try to connect to host
	cout << "Waiting for host announcement " << endl;

		// wait 1000 milli-secs for reply
		client = sock->wait(1000); // wait

		// since I am a client, client should = 0
		if( client == Socket::WAIT_PEER ) {
			// get the command from host
			num = sock->read((char *)rxBuf, CMD_MAX_SIZE);
			cout << "clientMode() - read " << num  << " CMD=" << (int)rxBuf[0] 
				<< " NUM=" << (int)rxBuf[2] << endl;

			if( rxBuf[0] == 127 )
				hostWait = 0;

		}
	}

	// try to connect to host
	cout << "Trying to connect to host" << endl;

	if( sock->connectHost() == false )
		perror("clientMode - connectHost could not connect");

	if( sock->isConnected() ) {
		// send to host
		uint8_t cmdBuf[] = {CMD_CONNECTING, 0, 0, 0};
		Command send(cmdBuf);
		send.setCheckSum();

		if( sock->write(send) != send.size() ){
			cout << "<!!! Could not send client connecting !!!>" <<endl; 
			return;
		}
	} else {
		cout << "Could not connect to host" << endl;
		return;
	}

	while( sock->isConnected() ) {

		// wait 1000 milli-secs for reply
		client = sock->wait(1000); // wait

		// since I am a client, client should = 0
		if( client == Socket::WAIT_PEER ) {
			// get the command from host
			num = sock->read((char *)rxBuf, CMD_MAX_SIZE);
			cout << "clientMode() - read " << num << endl;

			if( num > 0) {
				sockQueue.push(rxBuf,num);
				while( sockQueue.length() >= CMD_PACKET_SIZE && !sockQueue.needMoreBytes()) {
					if( sockQueue.isValid() ){
						Command rxCmd = sockQueue.popCommand();
						parseClientCommand(rxCmd);
					} else {
						cout << "-- Dropping byte --" << endl;
						(void)sockQueue.pop();
					}
				 }
			} else {
				if( num == 0) 
					cout << "Read 0 bytes" << endl;
				else if( errno != EWOULDBLOCK || errno != EINTR) 
					perror("clientMode() - bad read from host");

				sock->close();
			}       

		} else if( errno != EINTR && client != Socket::WAIT_TIMEOUT ) {
			perror("clientMode() - sock->wait()");
		}
	} // end of while()

	closeConnection();	
}


//----------------------------------------------------------
// Run like a server
//----------------------------------------------------------
void serverMode()
{
	Socket::SocketWait client;
	Command cmd;
	uint8_t *rxBuf = new uint8_t[ CMD_MAX_SIZE ];
	CommandQueue sockQueue;                         // parse incoming socket stream
	int num;

	// send time command
	uint8_t cmdBuf[CMD_MAX_SIZE] = { 127 };
	cmdBuf[0] = CMD_TIME_LOOP;
	cmdBuf[1] = CMD_MAX_SIZE - CMD_PACKET_SIZE;
	Command time(cmdBuf);
	time.setCheckSum();

	cmdBuf[0] = 254;
	cmdBuf[1] = 1;
	cmdBuf[2] = 1;
	Command hostB(cmdBuf);
	hostB.setCheckSum();


	int i = 0;
	//for(int i=0; i < NUM_LOOP; i++) {
	while(1)
	{
		// Broadcast to all that I am a VC
		if( Bsock->write(hostB) != hostB.size() ){
			cout << "<!!! Could not send VC broadcast !!!>" <<endl; 
		}

		if( sock->getNumClients() > 0 ) {
			if( i < NUM_LOOP ) {
				//cout << "sending time-loop" << endl;
				if( sock->write(time) != time.size() ){
					cout << "<!!! Could not send time-loop!!!>" <<endl; 
					break;
				}
				tsTx.stamp();

				i++;

			 } else {
				//cout << endl;
				//cout << "Average time = " << (float)(totalTime / (256.0*1000.0)) << "ms " <<endl;
				//cout << "minTime time = " << (float)(minTime / 1000.0) << "ms " <<endl;
				//cout << "maxTime time = " << (float)(maxTime / 1000.0) << "ms " <<endl;

				// save data to file
				//timeQueue.saveToFile("sockt.dat"); // save as milli-sec
				// flush out queue
				(void)timeQueue.Pop( NULL, NUM_LOOP);

				// reset count
				i = 0;
				minTime   = 999999999;
				maxTime   = 0;
				totalTime = 0;
			}
		}

		// wait for input, timeout at 1000 milli-sec
		client = sock->wait(1000);

		if( client == Socket::WAIT_TIMEOUT)
			;//cout << "sock->wait timedout" << endl;
		else if(client == Socket::WAIT_INT)
			cout << "sock->wait interrupted" << endl;
		else if(client == Socket::WAIT_ERROR)
			cout << "sock->wait error" << endl;
		else if( client == Socket::WAIT_PEER) {
			// TCP client connect
			if( sock->getType() == Socket::TCP ) {
				if( !sock->connectClient() )
					cout << "sock->wait could not connect client" << endl;

			// UDP client data
			} else  {
				num = sock->read( cmd );
				if( num >= CMD_HEADER_SIZE && num == cmd.size() )
					parseHostCommand(cmd, -1);
				else if( num >= 0)
					printf("sock->read() - funny read from client");
				else if( errno != EWOULDBLOCK || errno != EINTR) 
					perror("sock->read() - bad read from client");
			}       
		} else if( client > 0 ){
			// loop through all clients
			for( int i=0; i < sock->getNumClients(); i++ ) {
				if( client >> i & 0x1 ) {
					// get the command from client
					//num = sock->read(i, cmd);
					num = sock->read(i,(char *)rxBuf, CMD_MAX_SIZE);
					//cout << "read " << num << endl;
					if( num > 0) {
						sockQueue.push(rxBuf,num);
						while( sockQueue.length() >= CMD_PACKET_SIZE) {
							if( sockQueue.isValid() ){
								Command rxCmd = sockQueue.popCommand();
								parseHostCommand(rxCmd, i);
							} else {
								cout << "-- Dropping byte --" << endl
									<< (int)sockQueue.popByte() << endl;
							}
						 }

					} else {
						if( num == 0) 
							cout << "Read 0 bytes" << endl;
						else if( errno != EWOULDBLOCK || errno != EINTR) 
							perror("serverMode() - bad read from client");

						if(!sock->removeClient(i))
							cout << "<--- could not remove client -->" <<endl;
						else
							cout << "removed client " << endl;
					}       
				}
			}
		}
	} // end of while(1) 

	closeConnection();	
}


//----------------------------------------------------------
// close connection
//----------------------------------------------------------
void closeConnection() 
{
	uint8_t cmdBuf[] = { CMD_DISCONNECTING, 0, 0, 0};
	Command cmd(cmdBuf);
	cmd.setCheckSum();

	cout << "Closing connection ... ";

	// send disconnect command to host
	if( sock != NULL) {
		if( sock->isConnected() ) 
			sock->write( cmd );
		
		// close sock
		sock->close();

		// print socket statistics
		sock->printStats();
	}
	cout << "closed" << endl;
}

//----------------------------------------------------------
// Main function
//----------------------------------------------------------

#ifdef VXWORKS
int sockTest(char *host, char *port, int tcp)
#else
int main(int argc, char *argv[])
#endif
{
	string hostStr = "localhost";
	string portStr = "7070";
	Socket::SocketType type = Socket::UDP;
	Socket::SocketMode mode = Socket::CLIENT;

#ifndef VXWORKS
     // used for getopt to get otions from arguments
     extern int optind;
     //extern char * optarg;

     int c;
     while ((c=getopt(argc,argv,"hctu")) != EOF) switch( (char)c)
	{
          case 'h':
			mode = Socket::SERVER;
			break;
          case 'c':
			mode = Socket::CLIENT;
			break;
          case 't':
			type = Socket::TCP;
			break;
          case 'u':
			type = Socket::UDP;
			break;
          default:
               cout << "Unknown arg " << argv[optind - 1] << endl
              		<< "\tsockTest {-h | -c} {-t | -u} [ ip ] [ port ] " << endl;
               exit(EXIT_SUCCESS);
     }

	// only hostname
	if( optind + 1  ==  argc) {
		hostStr = string(argv[optind]);	
	} else if( optind + 2  ==  argc) {
		hostStr = string(argv[optind]);	
		portStr = string(argv[optind + 1]);	
     } else if( optind < argc){
		cout << "\tsockTest {-h | -c} {-t | -u} [ ip ] [ port ] " << endl;
		exit(EXIT_SUCCESS);
	}
#else
	if( host != NULL )
		hostStr = string( host );
	if( port != NULL )
		portStr = string( port );
	if( tcp == 1)
		type = Socket::TCP;
#endif

	// Catch Ctrl-c
	signal( SIGINT, exitsignal);

	// init sock
	sock = new Socket(hostStr.c_str(),portStr.c_str(), type); 
	if ( !sock->open( mode ) ) {
		cout << "Could not open sock" << endl;

		delete sock;
		return false;
	}

	Bsock = new Socket("127.0.0.255",portStr.c_str(), type); 
	if ( !Bsock->open( Socket::CLIENT ) ) 
		cout << "Could not open Bsock" << endl;

	// go into the appropriate mode
	// these don't return untill they are finished, 
	// i.e. the sock was closed
	if( mode == Socket::CLIENT )
		clientMode();
	else
		serverMode();

	//---------------------------------------------------------- 
	// Total program exit
	//---------------------------------------------------------- 
	delete sock;	
	cout << endl;
	exit(EXIT_SUCCESS); return EXIT_SUCCESS;      
}

//----------------------------------------------------------
// Act on received commands
//----------------------------------------------------------
void parseClientCommand(Command &cmd)
{
	if( !cmd.isValid() ) {
		cout << "<!!! parseCommand - invalid checksum !!!>\n";
		cmd.printCmd();

		return;
	}       

	switch( cmd.cmd() ) {

	// server sent time command
	case CMD_TIME_LOOP:
		//cout << "<--- Time-Loop --->" << endl;

		// simply send it back ASAP
		if( sock->write(cmd) != cmd.size() ){
			cout << "<!!! Could not send back time-loop !!!>" <<endl; 
			perror("sock->write" );
			return;
		}
		break;

	case CMD_CONNECTING:
		cout << "<--- Connected to " 
			<< sock->get_hostname()
			<< " --->" << endl << flush;
		break;

	// check if exiting
	// can only close the socket and try to open a new one
	case CMD_DISCONNECTING:
		cout << "<--- host has disconnected --->" << endl;
		break;

	default:
		cout << "<!!! Unknown command " << cmd.cmd() << " !!!>" << endl;
		break;
	}

}

void parseHostCommand(Command &cmd, int client) 
{
	if( !cmd.isValid() ) {
		cout << "<!!! parseCommand - invalid checksum !!!>\n";
		cmd.printCmd();

		return;
	}       

	switch( cmd.cmd() ) {
	// check if connecting
	case CMD_CONNECTING:
		cout << "<--- Request to connect --->" << endl;

		if( ! sock->isConnected() ) {
			if( sock->connectClient() ) {
				if( sock->getType() == Socket::UDP ) 
					cout << "UDP client connected" << endl;
				else
					cout << "TCP client connected" << endl;
			} else	
				cout << "<!!! could not connect client !!!>" << endl;
		}

		{
			// reply to client
			uint8_t cmdBuf[] = {CMD_CONNECTING, 0, 0, 0};
			Command send(cmdBuf);
			send.setCheckSum();

			if( sock->write(send) != send.size() ){
				cout << "<!!! Could not send client connecting !!!>" <<endl; 
				return;
			}
		}

		break;

	// check if exiting
	case CMD_DISCONNECTING:
		cout << "<--- Reqeust to disconnect --->" << endl;

		if( (client = sock->removeClient(client)) == -1 )
			cout << "<!!! could not remove client !!!>" <<endl;
		else
			cout << "Removed client " << client << endl;

		break;

	// client returned time loop
	// calculate the time diff
	case CMD_TIME_LOOP:
		//cout << "<--- Time-Loop --->" << endl;

		unsigned long deltaT;
		register float msec;

		tsRx.stamp();
		deltaT = tsRx - tsTx;
		maxTime = (deltaT > maxTime ? deltaT : maxTime);
		minTime = (deltaT < minTime ? deltaT : minTime);
		totalTime += deltaT;
		msec = deltaT / 1000.0;

		timeQueue.Push( (QueueType *)&(msec) ); // save as milli-sec
		break;

	default:
		cout << "Num=" << (int)(cmd.num()) << " : " << DefinedCommands[ cmd.num() ].name << endl;
		//cout << "<!!! Unknown command " << cmd.cmd() << " !!!>" << endl;
		break;
	}
}
